#include "pcl/common/transforms.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
class node : public rclcpp::Node
{
  public:
	void topic_input_callback(sensor_msgs::msg::PointCloud2 msg)
	{
		pcl::PointCloud<pcl::PointXYZ> pointcloud, output;
		// convert and transform
		pcl::fromROSMsg(msg, pointcloud);
		geometry_msgs::msg::TransformStamped tf;
		Eigen::Vector3f offset;
		Eigen::Quaternionf rotate;
		try
		{
			// transform
			tf = this->tf_buffer->lookupTransform(this->output_frame_id, msg.header.frame_id,
												  tf2::TimePointZero);
			offset.x() = tf.transform.translation.x;
			offset.y() = tf.transform.translation.y;
			offset.z() = tf.transform.translation.z;
			rotate.x() = tf.transform.rotation.x;
			rotate.y() = tf.transform.rotation.y;
			rotate.z() = tf.transform.rotation.z;
			rotate.w() = tf.transform.rotation.w;
			pcl::transformPointCloud(pointcloud, output, offset, rotate);
			// get lock and update
			if (this->input_mutex.try_lock_for(std::chrono::milliseconds(100)))
			{
				input_pcl[msg.header.frame_id] = output;
				RCLCPP_DEBUG(this->get_logger(), "point cloud with id %s updated",
							 msg.header.frame_id.c_str());
				this->input_mutex.unlock();
			}
			else
			{
				RCLCPP_DEBUG(this->get_logger(),
							 "try lock timeout, point cloud with id %s not updated",
							 msg.header.frame_id.c_str());
			}
		}
		catch (tf2::TransformException &e)
		{
			RCLCPP_INFO(this->get_logger(), e.what());
		}
	}
	void output_callback(void)
	{
		pcl::PointCloud<pcl::PointXYZ> out;
		sensor_msgs::msg::PointCloud2 msg;
		if (this->input_mutex.try_lock_for(std::chrono::milliseconds(100)))
		{
			// merge all points
			for (auto i : this->input_pcl)
			{
				out += i.second;
				RCLCPP_DEBUG(this->get_logger(), "point cloud %s merged", i.first.c_str());
			}
			// convert and publish
			pcl::toROSMsg(out, msg);
			this->pub_out->publish(msg);
			RCLCPP_DEBUG(this->get_logger(), "point cloud published");
		}
		else
		{
			RCLCPP_DEBUG(this->get_logger(), "try lock timeout, failed to merge and publish");
		}
	}
	node(std::string name) : Node(name)
	{
		this->declare_parameter("input_topics", std::vector<std::string>());
		this->get_parameter("input_topics", this->input_topics);

		this->declare_parameter("output_topic", "pcl_out");
		std::string param_pcl_output;
		this->get_parameter("output_topic", param_pcl_output);

		this->declare_parameter("output_freq", 10);
		int param_pcl_output_freq;
		this->get_parameter("output_freq", param_pcl_output_freq);

		this->declare_parameter("output_frame_id", this->output_frame_id);
		this->get_parameter("output_frame_id", this->output_frame_id);

		if (this->input_topics.size() <= 0)
		{
			RCLCPP_WARN(this->get_logger(), "no input topic spicified, exit");
			exit(EXIT_FAILURE);
		}
		// subscribe all input topics
		for (auto i : this->input_topics)
		{
			rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr ptr;

			this->sub_in.push_back(
				rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr());
			this->sub_in.back() = this->create_subscription<sensor_msgs::msg::PointCloud2>(
				i, 10, std::bind(&node::topic_input_callback, this, std::placeholders::_1));
			this->input_topics.push_back(i);
			RCLCPP_INFO(this->get_logger(), "added subscription to topic %s", i.c_str());
		}
		RCLCPP_INFO(
			this->get_logger(), "output topic: %s, output frame id: %s, output frequency: %d",
			param_pcl_output.c_str(), this->output_frame_id.c_str(), param_pcl_output_freq);

		this->tf_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
		this->tf_listener = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer);

		this->pub_out =
			this->create_publisher<sensor_msgs::msg::PointCloud2>(param_pcl_output, 10);
		this->pub_timer =
			this->create_wall_timer(std::chrono::milliseconds(1000 / param_pcl_output_freq),
									std::bind(&node::output_callback, this));
	}

  private:
	std::string output_frame_id = "pcl_link";
	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
		pub_out; // output pointcloud publisher
	rclcpp::TimerBase::SharedPtr pub_timer;
	std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
		sub_in;							   // list of input subscriptions
	std::vector<std::string> input_topics; // list of input topics
	std::map<std::string, pcl::PointCloud<pcl::PointXYZ>> input_pcl; // input point cloud buffer
	std::timed_mutex input_mutex; // mutex to protect the buffer above, in case of multi-thread
								  // executor or callback group

	std::unique_ptr<tf2_ros::Buffer> tf_buffer;
	std::shared_ptr<tf2_ros::TransformListener> tf_listener;
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto n = std::make_shared<node>("pcl_merge_node");
	rclcpp::spin(n);
	rclcpp::shutdown();
	return 0;
}
